/**
* @file NaviPlanner.h
* @author
* @brief 导航规划
* @version 1.0
* @date 2020-07-23
*
* @copyright Copyright (c) 2020
*
*/

#pragma once

#include "Communication/NavigateToPoseActionClient.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "data_struct/geometry/PointCloud.h"
#include "data_struct/base/DataSet.h"
#include "lib/WrapFuncs.h"
#include <lib/ipa_room_planning.h>
#include <mutex>
#include "std_msgs/msg/u_int8.hpp"

namespace behavior_controller
{

class NaviPlanner
{
public :
    NaviPlanner() {};

    void run(double &linear_vel, double &angular_vel, bool &finished);

    void calculateNewPose(geometry_msgs::msg::PoseStamped &pose);

    void SetNode(const rclcpp::Node::SharedPtr &ros_node, std::mutex *lock_,
                 std::shared_ptr<IpaRoomPlanning> &ipa_room_plan_);

    rclcpp::Node::SharedPtr &getNode() { return ros_node_; };

    bool isNewPose();

    void setMission(uint8_t mission_type_, geometry_msgs::msg::PoseStamped goal_pose_)
    {
        mission_type = mission_type_;
        goal_pose = goal_pose_;
        navigate_to_pose_action_client->setFlag(false);
        navigate_to_pose_action_client->setState(ActionStatus::UNKNOWN);
    };

    uint8_t getMission() { return mission_type; };

    void cancelGoal();

    std::shared_ptr<IpaRoomPlanning> ipaPlanner() { return ipa_room_plan; };

    void callback(std_msgs::msg::UInt8::ConstPtr state_msg);

    geometry_msgs::msg::PoseStamped goal_pose;

private:
    rclcpp::Node::SharedPtr ros_node_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_ = nullptr;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr follow_path_state_sub;
    std::shared_ptr<IpaRoomPlanning> ipa_room_plan = nullptr;
    double conform_time = 0;
    uint8_t conform_status = uint8_t(ActionStatus::UNKNOWN);
    uint8_t mission_type = 0;
    bool send_flag = false;
};

//static
extern NaviPlanner navi_planner; //实例化类

}